import smbus
import time
import serial


# ===== PCA9685 寄存器定义 =====
__SUBADR1 = 0x02
__SUBADR2 = 0x03
__SUBADR3 = 0x04
__MODE1 = 0x00
__PRESCALE = 0xFE
__LED0_ON_L = 0x06
__LED0_ON_H = 0x07
__LED0_OFF_L = 0x08
__LED0_OFF_H = 0x09

PCA9685_ADDRESS = 0x40
HORIZONTAL_SERVO_CHANNEL = 9
VERTICAL_SERVO_CHANNEL = 10
bus = smbus.SMBus(1)

DEBUG = True


def debug_print(*args):
    if DEBUG:
        print(*args)


def set_pwm_freq(freq):
    prescaleval = 25000000.0 / 4096.0 / float(freq) - 1.0
    prescale = int(prescaleval + 0.5)
    oldmode = bus.read_byte_data(PCA9685_ADDRESS, __MODE1)
    bus.write_byte_data(PCA9685_ADDRESS, __MODE1, (oldmode & 0x7F) | 0x10)
    bus.write_byte_data(PCA9685_ADDRESS, __PRESCALE, prescale)
    bus.write_byte_data(PCA9685_ADDRESS, __MODE1, oldmode)
    time.sleep(0.005)
    bus.write_byte_data(PCA9685_ADDRESS, __MODE1, oldmode | 0x80)


def set_pwm(channel, on, off):
    offset = channel * 4
    bus.write_byte_data(PCA9685_ADDRESS, __LED0_ON_L + offset, on & 0xFF)
    bus.write_byte_data(PCA9685_ADDRESS, __LED0_ON_H + offset, on >> 8)
    bus.write_byte_data(PCA9685_ADDRESS, __LED0_OFF_L + offset, off & 0xFF)
    bus.write_byte_data(PCA9685_ADDRESS, __LED0_OFF_H + offset, off >> 8)


def set_servo_angle(channel, angle):
    pulse_width_us = (angle / 180.0) * (2500 - 500) + 500
    pulse = int(pulse_width_us * 4096 / 20000)
    set_pwm(channel, 0, pulse)
    time.sleep(0.05)


def constrain(value, min_val, max_val):
    return max(min(value, max_val), min_val)


# ===== 云台控制参数 =====
HORIZONTAL_MIN = 20
HORIZONTAL_MAX = 160
VERTICAL_MIN = 30
VERTICAL_MAX = 150
center_threshold = 15

# PID 控制参数
kp_pan = 1.0
ki_pan = 0.01
kp_tilt = 1.0
ki_tilt = 0.01

integral_x = 0
integral_y = 0

# 平滑系数
alpha = 0.3  # 越大越灵敏，越小越稳定

# 初始化舵机角度
current_pan = 90
current_tilt = 90

# 接收缓冲区
buffer = b''

# 串口配置
ser = serial.Serial(
    port='/dev/ttyS0',
    baudrate=9600,
    timeout=1,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    bytesize=serial.EIGHTBITS
)

# 初始化PCA9685
set_pwm_freq(50)
set_servo_angle(HORIZONTAL_SERVO_CHANNEL, current_pan)
set_servo_angle(VERTICAL_SERVO_CHANNEL, current_tilt)
time.sleep(1)

debug_print("开始主控制循环...")


def parse_data(raw_data):
    try:
        data = raw_data.decode('utf-8', errors='ignore').strip()
        debug_print(f"接收到的数据: {data}")
        if data.startswith('$') and data.endswith('#'):
            coords = data[1:-1].split(',')
            if len(coords) == 2:  # 确保有逗号分隔的两个值
                try:
                    x = int(coords[0])
                    y = int(coords[1])
                    return x, y
                except ValueError as ve:
                    debug_print(f"转换整数失败: {ve}, 数据: {coords}")
            else:
                debug_print("数据格式不正确，缺少逗号分隔的两个值")
        else:
            debug_print("数据格式不正确，缺少$开头或#结尾")
    except Exception as e:
        debug_print(f"数据解析失败: {e}, 原始数据: {raw_data}")
    return None, None


try:
    while True:
        if ser.in_waiting > 0:
            buffer += ser.read(ser.in_waiting)
            if b'#' in buffer:
                parts = buffer.split(b'#', 1)
                raw_data = parts[0] + b'#'
                buffer = parts[1]

                x, y = parse_data(raw_data)

                if x is not None and y is not None:
                    debug_print(f"有效坐标: X={x}, Y={y}")

                    # 应用低通滤波
                    x_filtered = alpha * x + (1 - alpha) * 0
                    y_filtered = alpha * y + (1 - alpha) * 0

                    # 水平控制
                    if abs(x_filtered) > center_threshold:
                        integral_x += x_filtered
                        integral_x = constrain(integral_x, -500, 500)
                        pan_adjust = x_filtered * kp_pan + integral_x * ki_pan
                        new_pan = current_pan - pan_adjust
                        current_pan = constrain(new_pan, HORIZONTAL_MIN, HORIZONTAL_MAX)
                        set_servo_angle(HORIZONTAL_SERVO_CHANNEL, current_pan)
                        debug_print(f"水平调整: {pan_adjust:.1f} -> 当前角度: {current_pan:.1f}")

                    # 垂直控制
                    if abs(y_filtered) > center_threshold:
                        integral_y += y_filtered
                        integral_y = constrain(integral_y, -500, 500)
                        tilt_adjust = y_filtered * kp_tilt + integral_y * ki_tilt
                        new_tilt = current_tilt - tilt_adjust
                        current_tilt = constrain(new_tilt, VERTICAL_MIN, VERTICAL_MAX)
                        set_servo_angle(VERTICAL_SERVO_CHANNEL, current_tilt)
                        debug_print(f"垂直调整: {tilt_adjust:.1f} -> 当前角度: {current_tilt:.1f}")

                else:
                    debug_print("丢弃无效数据")
        else:
            # 若长时间无数据，缓慢归中
            if current_pan != 90 or current_tilt != 90:
                delta_pan = (90 - current_pan) * 0.5
                delta_tilt = (90 - current_tilt) * 0.5
                current_pan += delta_pan
                current_tilt += delta_tilt
                current_pan = constrain(current_pan, HORIZONTAL_MIN, HORIZONTAL_MAX)
                current_tilt = constrain(current_tilt, VERTICAL_MIN, VERTICAL_MAX)
                set_servo_angle(HORIZONTAL_SERVO_CHANNEL, int(current_pan))
                set_servo_angle(VERTICAL_SERVO_CHANNEL, int(current_tilt))
                debug_print(f"归中模式: 水平={int(current_pan)}, 垂直={int(current_tilt)}")
            time.sleep(0.1)

except KeyboardInterrupt:
    print("程序终止")
finally:
    for ch in range(16):
        set_pwm(ch, 0, 0)
